#include "improvedVerlet.h"

ImprovedVerlet::ImprovedVerlet() : IntegrationMethod()
{
    eta = 0.01;
}

ImprovedVerlet::ImprovedVerlet(double _deltaT, double _g, std::vector<State> _s0,
                               std::vector< std::vector<Particle *> > &_p, std::vector< std::vector<Spring *> > &_s,
                               bool _activeAir, int _airType, double _airPeriod, double _airRepeat, double _airViscosity, Vector3D _airDirection)
                               : IntegrationMethod(_deltaT, _g, _s0, _p, _s, _activeAir, _airType, _airPeriod, _airRepeat, _airViscosity, _airDirection)
{
    eta = 0.01;
}

ImprovedVerlet::~ImprovedVerlet()
{
    Vector *v, *p;
    for(unsigned int i = 0; i < previousResultantForce.size(); i++)
    {
        v = previousResultantForce.at(i);
        p = resultantForce.at(i);
        delete v;
        delete p;
    }
}

void ImprovedVerlet::calculate(bool _firstIteration, unsigned int _fabricPosition, double _fraction)
{
    //guardando dados
    State s = currentState.at(_fabricPosition);
    double t = s.getT();
    unsigned int dof = s.getDOF();
    Matrix reference = s.getReferenceMatrix();
    std::vector< std::pair<bool, Object *> > collision = s.getCollision();


    previousResultantForce.clear();

    previousResultantForce.insert(previousResultantForce.begin(), resultantForce.begin(), resultantForce.end());


     //guardando dados

    currentDeltaT = deltaT / _fraction;


    std::vector<Particle *> particlesI = particles.at(_fabricPosition);

    //calculando
    if(_firstIteration)
    {

        s = currentState.at(_fabricPosition) + (currentDeltaT * f(currentState.at(_fabricPosition), _fabricPosition));

    }
    else
    {

        for(unsigned int j=0; j<particlesI.size(); j++)
        {

            if((reference[j][0] == -1) && (reference[j][1] == -1) && (reference[j][2] == -1)) continue;


            double mass = particlesI.at(j)->getMass();


            Vector3D currentForce3D, previousForce3D, currentPosition, previousPosition, currentVelocity;
            int place;

            currentState.at(_fabricPosition).getParticlePositionVelocity(j, particlesI.at(j)->getPosition(), particlesI.at(j)->getVelocity(), &currentPosition, &currentVelocity);

            previousPosition = previousState.at(_fabricPosition).getParticlePosition(j, particlesI.at(j)->getPosition());


            for(unsigned int k=0; k<3; k++)
            {

                place = reference[j][k];

                if (place != -1)
                {

                    previousForce3D[k] = (*previousResultantForce.at(_fabricPosition))[place];

                }

            }
            currentForce3D = particleResultanteForce(currentState.at(_fabricPosition), currentPosition, currentVelocity, _fabricPosition, j);

            if ((Vector(currentForce3D - previousForce3D)).length() < eta)
            {
                verlet(&s, mass, currentForce3D, currentPosition, previousPosition, _fabricPosition, j);
            }
            else
            {
                beemanAlgorithm(&s, mass, currentForce3D, currentPosition, currentVelocity, _fabricPosition, j);
            }

        }
    }


    //atualizando
    previousState.at(_fabricPosition) = currentState.at(_fabricPosition);
    currentState.at(_fabricPosition) = s;
    currentState.at(_fabricPosition).setT_DOF_Ref_Col(t + currentDeltaT, dof, reference, collision);

}

//usado somente na primeira interação
State ImprovedVerlet::f(const State &_state, unsigned int _i)
{
    Vector a = acceleration(_state, _i);
    previousAcceleration.push_back(a);

    return State (_state.getVelocity() + a.scalarProduct(currentDeltaT/2.0), a);
}

//usado somente na primeira interação
Vector ImprovedVerlet::acceleration(const State &_state, unsigned int _i)
{
    Matrix *m = populateM(_state, _i);

    Vector *rf = populateResultantForces(_state, _i);
    resultantForce.push_back(rf);

    Vector acc = (*m).diagonalInverse() * (*rf);

    delete m;
    return acc;
}

Vector3D ImprovedVerlet::particleResultanteForce(const State &_state, const Vector3D &_position, const Vector3D &_velocity, unsigned int _i, unsigned int _j)
{
    const Matrix &ref = _state.getReferenceMatrix();
    std::vector<Particle *> particlesI = particles.at(_i);
    std::vector<Spring *> springsI = springs.at(_i);

    Vector3D rf, gf, ef, df,cf, dragf, airf;

    //força gravitacional
    if (ref[_j][2] != -1) gf[2] = particlesI.at(_j)->getMass() * gravity;

    std::pair<bool, Object *> collision = _state.getCollision(_j);
    if(collision.first)
    {

        Vector3D normal = collision.second->getNormalAt(_position);
        Vector3D normalComponent = normal*(_velocity.dotProduct(normal));
        Vector3D tangentialComponent = _velocity - normalComponent;
        for(int j = 0; j < 3; j++)
        {
            int place = ref[_j][j];
            if (place != -1) cf[j] = (normalComponent[j] * particlesI.at(_j)->getMiC())
                                       + (tangentialComponent[j] * collision.second->getMi(_velocity));
        }
    }

    //força elastica e damping
    unsigned int size = springsI.size();
   // #pragma omp parallel for //schedule(dynamic)
    for(unsigned int i = 0; i < size; i++)
    {
        Spring *s = NULL;
        double lc;
        Vector3D positionBegin, positionEnd;
        Vector3D velocityBegin, velocityEnd;

        s = springsI.at(i);
        if(!(s->getParticleBegin() == (int)_j || s->getParticleEnd() == (int)_j)) continue;

        if(s->getParticleBegin() == (int)_j)
        {

            positionBegin = _position;
            positionEnd = _state.getParticlePosition(s->getParticleEnd(), particlesI.at(s->getParticleEnd())->getPosition());

        }
        else
        {
            positionEnd = _position;
            positionBegin = _state.getParticlePosition(s->getParticleBegin(), particlesI.at(s->getParticleBegin())->getPosition());

        }

        lc = s->getCurrentLength(positionBegin, positionEnd);


        {
            Vector3D force;
            double ks, lo;
            int currentDOF;
            bool delta;

            delta = s->positiveDelta();
            if ((s->getType() == FLEXION && !delta) || (s->getType() != FLEXION && delta))
            {
                ks = s->getKs();
                lo = s->getOriginalLength();

                force = ( positionEnd - positionBegin ) * ( (ks * (lc - lo)) / lc );

                if(s->getParticleBegin() == (int)_j)
                {
                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleBegin()][j];
                        if( currentDOF != -1 ) ef[j] += force[j];
                    }
                }
                else
                {
                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleEnd()][j];
                        if( currentDOF != -1 ) ef[j] -= force[j];
                    }
                }
            }
        }

        {
            Vector3D force;
            int currentDOF;
            bool delta;

            delta = s->positiveDelta();

            Vector3D velocityBegin, velocityEnd;
            double kd;

            if(s->getParticleBegin() == (int)_j)
            {
                velocityBegin = _velocity;
                velocityEnd = _state.getParticleVelocity(s->getParticleEnd(), particlesI.at(s->getParticleEnd())->getVelocity());
            }
            else
            {
                velocityEnd = _velocity;
                velocityBegin = _state.getParticleVelocity(s->getParticleBegin(), particlesI.at(s->getParticleBegin())->getVelocity());
            }
            if ((s->getType() == FLEXION && !delta) || (s->getType() != FLEXION && delta))
            {

                kd = s->getKd();

                force = (positionEnd - positionBegin) * ((kd * ((velocityEnd - velocityBegin).dotProduct(positionEnd - positionBegin))) / (lc * lc));
                //std::cout<<"força de amortecimento ["<<force.x<<","<<force.y<<","<<force.z<<" ]" <<std::endl;

                if(s->getParticleBegin() == (int)_j)
                {
                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleBegin()][j];
                        if( currentDOF != -1 ) df[j] += force[j];
                    }
                }
                else
                {
                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleEnd()][j];
                        if( currentDOF != -1 ) df[j] -= force[j];
                    }
                }
            }
        }
    }

     //drag force of air and  wind force

    Vector3D fluidVelocity = velocityInTime(_state.getT());

    {
        Particle *particle = particlesI.at(_j);

        double _kd = (*particle).getKd();

        Vector3D velocityParticle = _velocity;

        Vector3D dragForce = velocityParticle * (_kd);

        Vector3D airForce;

        if(useAir)
        {
            Vector3D normal = _state.getParticleNormal(particle);

            airForce = normal * (fluidViscosity * (normal.dotProduct(fluidVelocity - velocityParticle)));
        }

        int currentDOF;

        for(int j = 0; j < 3; j++)
        {
            currentDOF = ref[_j][j];

            if( currentDOF != -1 )
            {
                dragf[j] -= dragForce[j];

                if(useAir) airf[j] += airForce[j];

            }
        }

    }

    rf = ef + df + gf + cf + dragf;

    if(useAir) rf = rf + airf;

    return rf;

}

Vector3D ImprovedVerlet::particleAcceleration(double _mass, const Vector3D &_resultantForce)
{

    return _resultantForce * (1.0/_mass);
}

void ImprovedVerlet::verlet(State *_state, double _mass, Vector3D _resultantForce, const Vector3D &_currentPosition, const Vector3D &_previousPosition, unsigned int _i, unsigned int _j)
{

    Vector3D a = particleAcceleration(_mass, _resultantForce);

    Matrix reference = currentState.at(_i).getReferenceMatrix();

    Vector3D position = (_currentPosition * 2.0) - _previousPosition + (a * (currentDeltaT * currentDeltaT));

    Vector3D velocity = (position - _currentPosition) * (1.0/currentDeltaT);


    int place;
    for(unsigned int k=0; k<3; k++)
    {
        place = reference[_j][k];

        if (place != -1)
        {
            previousAcceleration.at(_i)[place] = a[k];
            (*resultantForce.at(_i))[place] = _resultantForce[k];
            _state->getPosition()[place] = position[k];
            _state->getVelocity()[place] = velocity[k];
        }
    }

}


void ImprovedVerlet::beemanAlgorithm(State *_state, double _mass, Vector3D _resultantForce, const Vector3D &_currentPosition, const Vector3D &_currentVelocity, unsigned int _i, unsigned int _j)
{

    Matrix reference = currentState.at(_i).getReferenceMatrix();

    int place;
    Vector3D previousA;
    for(unsigned int k=0; k<3; k++)
    {
        place = reference[_j][k];

        if (place != -1)
        {
            previousA[k] = previousAcceleration.at(_i)[place];
        }
    }


    Vector3D a = particleAcceleration(_mass, _resultantForce);


    Vector3D position = _currentPosition + (_currentVelocity * currentDeltaT) + (((a * 4.0) - previousA) * ((currentDeltaT * currentDeltaT)/6.0));
    Vector3D velocity = _currentVelocity + (a * ((3.0 * currentDeltaT)/2.0)) - (previousA * (currentDeltaT/2.0));



    Vector3D newVelocity = corrector(particleResultanteForce(*_state, position, velocity, _i, _j), _mass, _currentVelocity, previousA, a);


    for(unsigned int k=0; k<3; k++)
    {
        place = reference[_j][k];

        if (place != -1)
        {
            previousAcceleration.at(_i)[place] = a[k];
            (*resultantForce.at(_i))[place] = _resultantForce[k];
            _state->getPosition()[place] = position[k];
            _state->getVelocity()[place] = newVelocity[k];
        }
    }
}

Vector3D ImprovedVerlet::corrector(const Vector3D& _newResultantForce, double _mass, const Vector3D &_currentVelocity, const Vector3D &_previousAcceleration, const Vector3D & _currentAcceleration)
{

    Vector3D newA = particleAcceleration(_mass, _newResultantForce);

    Vector3D velocity = _currentVelocity + ((newA * 2.0) + (_currentAcceleration * 5.0) - _previousAcceleration) * (currentDeltaT/6.0);

    return velocity;

}
